#include "robot_manager.h"

void arrayCallback(const std_msgs::Float64MultiArray::ConstPtr& msg)
{
  ROS_INFO("Received Float64MultiArray:");
  for (size_t i = 0; i < msg->data.size(); ++i)
  {
    ROS_INFO("  data[%lu] = %f", i, msg->data[i]);
  }
}

RobotManager::RobotManager(ros::NodeHandle &nh) :
    nh_(nh), joint_pub_(nh_.advertise<sensor_msgs::JointState>("/joint_states", 10)), cmd_pub_(nh_.advertise<sensor_msgs::JointState>("/cmd_states", 10))
{

    
    ZLogInit(ELOG_LVL_INFO, "./build/log", 50);

    robot_model_ = std::make_shared<ZORAL::ZoralModel>("zoram_one_alpha", "/opt/zoral/data/");
    robot_plan_ = std::make_shared<ZORAL::ZoralPlan>(robot_model_);

    //! 从参数服务器读取 is_simulation 参数，默认为 true
    nh_.param("is_simulation", is_simulation_, true);
    ROS_INFO("init done.");

    if(!is_simulation_)
    {
        initializeMotors();
    }
}

void RobotManager::start()
{
    joint_state_msg_.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"};
    joint_state_msg_.position = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    joint_state_msg_.velocity = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    joint_state_msg_.effort = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    cmd_msg_.name = joint_state_msg_.name;
    cmd_msg_.position = joint_state_msg_.position;
    cmd_msg_.velocity = joint_state_msg_.velocity;
    cmd_msg_.effort = joint_state_msg_.effort;
    if (is_simulation_)
    {
        ROS_INFO("RobotManager is running in SIMULATION mode.");

        //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
        joint_state_msg_.header.stamp = ros::Time::now();
        joint_pub_.publish(joint_state_msg_); // 发布初始的 joint_states 消息
    }
    else
    {
        ROS_INFO("RobotManager is running in REAL-HARDWARE mode.");
    }
    
    running_ = true;
    joint_state_refresh_thread_ = std::thread(&RobotManager::publishJointStatesLoop, this);

    move_joint_subscriber_ = nh_.subscribe("/command_move_joint", 1000, &RobotManager::commandMoveJointCallback, this);
    move_pose_subscriber_ = nh_.subscribe("/command_move_pose", 1000, &RobotManager::commandMovePoseCallback, this);
    move_traj_subscriber_ = nh_.subscribe("/command_move_traj", 1000, &RobotManager::commandMoveTrajCallback, this);
    move_line_subscriber_ = nh_.subscribe("/command_move_line", 1000, &RobotManager::commandMoveLineCallback, this);
    move_nullspace_subscriber_ = nh_.subscribe("/command_move_nullspace", 1000, &RobotManager::commandMoveNullspaceCallback, this);
    teach_move_subscriber_ = nh_.subscribe("/command_teach_move", 1000, &RobotManager::commandTeachMoveCallback, this);
    trajectory_replay_subscriber_ = nh_.subscribe("/command_trajectory_replay", 1000, &RobotManager::commandTrajectoryReplayCallback, this);
    joint_scan_subscriber_ = nh_.subscribe("/command_joint_scan", 1000, &RobotManager::commandJointScanCallback, this);
}

